#ifndef _TRANSFORM_H
#define _TRANSFORM_H

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Core>
#include <Eigen/Dense>


namespace transform{

    void Mat4d2Pose(Eigen::Matrix4d& TMat,geometry_msgs::Pose& ros_pose)
    {
        ros_pose.position.x = TMat(0,3);
        ros_pose.position.y = TMat(1,3);
        ros_pose.position.z = TMat(2,3);

        Eigen::Matrix3d R_mat = TMat.block<3,3>(0,0);
        Eigen::Quaterniond q(R_mat);
        ros_pose.orientation.w = q.w();
        ros_pose.orientation.x = q.x();
        ros_pose.orientation.y = q.y();
        ros_pose.orientation.z = q.z();
    }

    void Pose2Mat4d(Eigen::Matrix4d& TMat,geometry_msgs::Pose& ros_pose)
    {
        TMat = Eigen::Matrix4d::Identity();
        TMat(0,3) = ros_pose.position.x;
        TMat(1,3) = ros_pose.position.y;
        TMat(2,3) = ros_pose.position.z;

        Eigen::Quaterniond q(ros_pose.orientation.w,ros_pose.orientation.x,ros_pose.orientation.y,ros_pose.orientation.z);
        Eigen::Matrix3d R_mat = q.toRotationMatrix();
        TMat.block<3,3>(0,0) = R_mat;
    }

    void MatSlerp(Eigen::Matrix4d& out,Eigen::Matrix4d& TMat_1,Eigen::Matrix4d& TMat_2,double percent)
    {
        Eigen::Matrix3d R_1 = TMat_1.block<3,3>(0,0);
        Eigen::Matrix3d R_2 = TMat_2.block<3,3>(0,0);

        Eigen::Quaterniond q1(R_1);
        Eigen::Quaterniond q2(R_2);

        q1.slerp(percent,q2);

        out = Eigen::Matrix4d::Identity();
        out.block<3,3>(0,0) = q1.toRotationMatrix();
        out(0,3) = TMat_1(0,3)+percent*(TMat_2(0,3)-TMat_1(0,3));
        out(1,3) = TMat_1(1,3)+percent*(TMat_2(1,3)-TMat_1(1,3));
        out(2,3) = TMat_1(2,3)+percent*(TMat_2(2,3)-TMat_1(2,3));        
    }

    void PoseStampedSlerp(double target_time,geometry_msgs::PoseStamped& source_1,geometry_msgs::PoseStamped& source_2)
    {
        double percent = (target_time-source_1.header.stamp.toSec())/(source_2.header.stamp.toSec()-source_1.header.stamp.toSec());
        Eigen::Matrix4d T1,T2;
        Pose2Mat4d(T1,source_1.pose);
        Pose2Mat4d(T2,source_2.pose);
        Eigen::Matrix4d after_slerp;
        MatSlerp(after_slerp,T1,T2,percent);

        Mat4d2Pose(after_slerp,source_1.pose);
        source_1.header.stamp = ros::Time().fromSec(source_1.header.stamp.toSec()+percent*(source_2.header.stamp.toSec()-source_1.header.stamp.toSec()));
    }

    void OdometrySlerp(double target_time,nav_msgs::Odometry& source_1,nav_msgs::Odometry& source_2)
    {
        double percent = (target_time-source_1.header.stamp.toSec())/(source_2.header.stamp.toSec()-source_1.header.stamp.toSec());
        Eigen::Matrix4d T1,T2;
        Pose2Mat4d(T1,source_1.pose.pose);
        Pose2Mat4d(T2,source_2.pose.pose);
        Eigen::Matrix4d after_slerp;
        MatSlerp(after_slerp,T1,T2,percent);

        Mat4d2Pose(after_slerp,source_1.pose.pose);
        double slerp_time = source_1.header.stamp.toSec()+percent*(source_2.header.stamp.toSec()-source_1.header.stamp.toSec());
        source_1.header.stamp = ros::Time().fromSec(slerp_time);
    }

}


#endif